
#include "ulti.h"
#include "path.h"
#include<string>

/*
#define LEFTTOP_CORNER 4369
#define RIGHTTOP_CORNER 61440
#define LEFTBUTTON_CORNER 34952
#define RIGHTBUTTON_CORNER 15*/

#define LEFTTOP_CORNER 62297
#define RIGHTTOP_CORNER  63903
#define LEFTBUTTON_CORNER 1632
#define RIGHTBUTTON_CORNER 42405

#define MIDBOT_MARKER 40953
#define BLOCKED_MARKER 36840
#define TARGET_MARKER 65535
#define EXIT_MARKER 61455
#define OBJECT_MARKER 26214

#define PATH_SINGLEMODE 0
#define PATH_CROSSMODE 1


#define CENTER_OFFSET 0.5f

#define MAP_TO_MAP2_ANGLE 315
#define MAP_TO_MAP2_DISTANCE 600
using namespace cv;


//global variables//
CvCapture *capture ;
CvMat *imageToMap;
CvCapture *capture2 ;
CvMat *imageToMap2;
//                     //

void markerdetector(IplImage * capimg,IplImage * mapimg,CvMat* matrix){
	CvFont dfont;
	float hscale      = 0.5f;
	float vscale      = 0.5f;
	float italicscale = 0.0f;
	int  thickness    = 1;
	char text[255] = "";
	cvInitFont (&dfont, CV_FONT_HERSHEY_SIMPLEX , hscale, vscale, italicscale, thickness, CV_AA);

	vector<shared_ptr<Marker>> markers;
	detectMarker(capimg,markers);
	for(int i=0;i<markers.size();i++){
		cvDrawContours(capimg,markers.at(i)->contour,CV_RGB(255,255,0),CV_RGB(200,255,255),0,1);
		if(markers.at(i)->id==MIDBOT_MARKER){
		    CvPoint *head=(CvPoint*)(cvGetSeqElem(markers.at(i)->contour,(markers.at(i)->rotation/90)%4));
			CvPoint *tail=(CvPoint*)(cvGetSeqElem(markers.at(i)->contour,(markers.at(i)->rotation/90+3)%4));
			cvLine(capimg,cvPoint((int)head->x,(int)head->y),cvPoint((int)tail->x,(int)tail->y),CV_RGB(255,0,0),2);

			CvPoint obj_ctr_map=perspectiveTransform_1pnt(cvPoint(markers.at(i)->x,markers.at(i)->y),matrix);
			CvPoint obj_head_map=perspectiveTransform_1pnt(*head,matrix);
			CvPoint obj_tail_map=perspectiveTransform_1pnt(*tail,matrix);
			obj_ctr_map.x+=(obj_head_map.x-obj_tail_map.x)*CENTER_OFFSET;
			obj_ctr_map.y+=(obj_head_map.y-obj_tail_map.y)*CENTER_OFFSET;
			cvLine(mapimg,obj_head_map,obj_tail_map,CV_RGB(255,0,0),2);
			cvDrawCircle(mapimg,cvPoint((int)obj_ctr_map.x,(int)obj_ctr_map.y),4,CV_RGB(255,0,0),3,8,0);
			float angle=calculateAngle(cvPoint(0,0),cvPoint(0,400),obj_head_map,obj_tail_map);
			sprintf(text,"x=%d,y=%d",obj_ctr_map.x,obj_ctr_map.y);
			cvPutText(mapimg,text,cvPoint(10,20),&dfont,CV_RGB(255,255,255));
			sprintf(text,"angle=%f",angle);
			cvPutText(mapimg,text,cvPoint(10,40),&dfont,CV_RGB(255,255,255));

		}
		cvDrawCircle(capimg,cvPoint(cvRound(markers.at(i)->x),cvRound(markers.at(i)->y)),2,CV_RGB(255,0,0),3,8,0);
		sprintf(text,"%d",markers.at(i)->id);
		cvPutText(capimg,text,cvPoint((int)markers.at(i)->x,(int)markers.at(i)->y),&dfont,CV_RGB(255,0,0));
	}
}

void buildGridMap(IplImage* img,GridMap& gridmap,CvMat* matrix,int mode){
	vector<shared_ptr<Marker>> markers;
	detectMarker(img,markers);
	for(int i=0;i<markers.size();i++){
		int x,y;
		CvPoint mappoint=perspectiveTransform_1pnt(cvPoint(markers.at(i)->x,markers.at(i)->y),matrix);
		switch(markers.at(i)->id){
		case MIDBOT_MARKER:
			gridmap.setGrid(mappoint.x/100,mappoint.y/100,GRIDMAP_CONT_START);
			break;
		case BLOCKED_MARKER:
			gridmap.setGrid(mappoint.x/100,mappoint.y/100,GRIDMAP_CONT_BLOCKED);
			break;
		case TARGET_MARKER:
			if(mode==PATH_SINGLEMODE)
				gridmap.setGrid(mappoint.x/100,mappoint.y/100,GRIDMAP_CONT_END);
			break;
		case EXIT_MARKER:
			if(mode==PATH_CROSSMODE)
				gridmap.setGrid(mappoint.x/100,mappoint.y/100,GRIDMAP_CONT_END);
			break;

		}
	}
};

void grabObject(int markerid,CvCapture* capture){//demo for grab an object and place it to the next grid
	while(1){
		bool found=false;
		shared_ptr<Marker> marker;
		IplImage *img;
		for(int j=0;j<3;j++){
			//for(int i=0;i<10;i++){
			IplImage *img=cvQueryFrame(capture);
			found=findAMarker(img,OBJECT_MARKER,marker);
			if(found)break;
		}
		if(found){
			if(abs(marker->x-320)<60){
				if(cvContourPerimeter(marker->contour)>600)
				{
					if(cvContourPerimeter(marker->contour)<700){
						Forward(0.01);
					}
					else{
						Forward(0.25);
						cout<<"reach!"<<endl;
						break;
					}
				}
				else Forward(0.05);
			}
			else{
				if(marker->x>320)TurnRight(0.01);
				else TurnLeft(0.01);
			}
			cout<<"marker_x="<<marker->x<<" contourPerimeter="<<cvContourPerimeter(marker->contour)<<endl;
			cvQueryFrame(capture);
			cvQueryFrame(capture);
			cvQueryFrame(capture);

		}
		else{
			if(cvWaitKey(1000)==27)return;
		}
	}

	Up();
	Close();
	Down();
	Back(1);
	Up();
	Open();
	Down();



	
}

bool detectObstacle(){
	//Sleep(5000);
	TurnLeft(0.25);
	bool foundObstacle=false;
	for(int k=0;k<4;k++){
		bool found=false;
		shared_ptr<Marker> marker;
		IplImage *img;

		for(int i=0;i<10;i++){
			IplImage *img=cvQueryFrame(capture);
			if(img==NULL){
				cout<<"fail to get frame"<<endl;
				continue;
			}
			found=findAMarker(img,OBJECT_MARKER,marker);
			if(found)break;
		}
		if(found){
			cout<<"marker_x="<<marker->x<<" contourPerimeter="<<cvContourPerimeter(marker->contour)<<endl;
			cvQueryFrame(capture);cvQueryFrame(capture);cvQueryFrame(capture);
			if(cvContourPerimeter(marker->contour)>280)foundObstacle=true;
		}
		TurnRight(0.05);
	}
	return foundObstacle;
}
	//}


void calibrateAngle(shared_ptr<Marker> marker,CvMat* matrix,int direction){
	float angle;
	CvPoint *head=(CvPoint*)(cvGetSeqElem(marker->contour,(marker->rotation/90)%4));
	CvPoint *tail=(CvPoint*)(cvGetSeqElem(marker->contour,(marker->rotation/90+3)%4));
	CvPoint obj_ctr_map=perspectiveTransform_1pnt(cvPoint(marker->x,marker->y),matrix);
	CvPoint obj_head_map=perspectiveTransform_1pnt(*head,matrix);
	CvPoint obj_tail_map=perspectiveTransform_1pnt(*tail,matrix);
	obj_ctr_map.x+=(obj_head_map.x-obj_tail_map.x)*CENTER_OFFSET;
	obj_ctr_map.y+=(obj_head_map.y-obj_tail_map.y)*CENTER_OFFSET;
	CvPoint to=cvPoint(obj_ctr_map.x,obj_ctr_map.y);
	switch(direction){
	case 0:
		to.y+=10;
		break;
	case 1:
		to.x+=10;
		break;
	case 2:
		to.y-=10;
		break;
	case 3:
		to.x-=10;
		break;
	}
	angle=calculateAngle(obj_tail_map,obj_head_map,to,obj_ctr_map);
	trunAngle(angle);
}

void capAndCal(CvCapture* cap,CvMat* matrix,int direction){
	cvQueryFrame(cap);
	cvQueryFrame(cap);
	cvQueryFrame(cap);
	shared_ptr<Marker> marker;
	bool found=false;
	for(int i=0;i<10;i++){
		IplImage *img=cvQueryFrame(cap);
		found=findAMarker(img,MIDBOT_MARKER,marker);
		if(found)break;
	}
	if(found){
		calibrateAngle(marker,matrix,direction);
	}
}
void runAStep(shared_ptr<Marker> marker,CvPoint step,CvMat* matrix){
	float distance,angle;
	CvPoint *head=(CvPoint*)(cvGetSeqElem(marker->contour,(marker->rotation/90)%4));
	CvPoint *tail=(CvPoint*)(cvGetSeqElem(marker->contour,(marker->rotation/90+3)%4));
	CvPoint obj_ctr_map=perspectiveTransform_1pnt(cvPoint(marker->x,marker->y),matrix);
	CvPoint obj_head_map=perspectiveTransform_1pnt(*head,matrix);
	CvPoint obj_tail_map=perspectiveTransform_1pnt(*tail,matrix);
	obj_ctr_map.x+=(obj_head_map.x-obj_tail_map.x)*CENTER_OFFSET;
	obj_ctr_map.y+=(obj_head_map.y-obj_tail_map.y)*CENTER_OFFSET;
	howToAPoint(obj_ctr_map,cvPoint(step.x*100+50,step.y*100+50),obj_head_map,obj_tail_map,angle,distance);
	cout<<"target:"<<step.x*100+50<<" "<<step.y*100+50<<endl;
	cout<<"from:"<<obj_ctr_map.x<<" "<<obj_ctr_map.y<<endl;
	cout<<"how to go: angle="<<angle<<" distance="<<distance<<endl;
	goToAPoint(angle,distance);
}


void onMouse( int event, int x, int y, int, void* ){
	if(event==CV_EVENT_LBUTTONDOWN ){//code demo for single space with single/multiple camera(s)
		GridMap gridmap(4,5);
		buildGridMap(cvQueryFrame(capture),gridmap,imageToMap,PATH_SINGLEMODE);
		//buildGridMap(cvQueryFrame(capture2),gridmap,imageToMap2);//for multicams in the same space
		gridmap.printContent();
		gridmap.findPath();
		vector<CvPoint> path;
		gridmap.getPath(path);
		if(gridmap.isValidMap())cout<<"[Path]"<<endl;
		else cout<<"path not found";
		for(int i=0;i<path.size();i++){
			cout<<path.at(i).x<<","<<path.at(i).y<<endl;
		}
		cout<<"[Press Key to go]"<<endl;
		char ch;
		cin>>ch;
		for(int i=0;i<path.size();i++){
			shared_ptr<Marker> marker;
			bool found=false;
			for(int i=0;i<10;i++){
				IplImage *img=cvQueryFrame(capture);
				found=findAMarker(img,MIDBOT_MARKER,marker);
				if(found)break;
			}
			if(found){
				runAStep(marker,path.at(i),imageToMap);

			}
			cvQueryFrame(capture);
			cvQueryFrame(capture);
			cvQueryFrame(capture);
		}
	}
	if(event==CV_EVENT_RBUTTONDOWN){
		//path planning with obstacle avoidance
		vector<CvPoint>obstacles;
		bool reach=false;
		while(!reach){
			cvQueryFrame(capture2);
			cvQueryFrame(capture2);
			cvQueryFrame(capture2);
			GridMap gridmap(4,5);
			for(int i=0;i<obstacles.size();i++){
				gridmap.setGrid(obstacles.at(i).x,obstacles.at(i).y,GRIDMAP_CONT_BLOCKED);
			}
			buildGridMap(cvQueryFrame(capture2),gridmap,imageToMap2,PATH_SINGLEMODE);

			gridmap.printContent();
			gridmap.findPath();
			vector<CvPoint> path;
			gridmap.getPath(path);
			if(gridmap.isValidMap())cout<<"[Path]"<<endl;
			else cout<<"path not found";
			for(int i=0;i<path.size();i++){
				cout<<path.at(i).x<<","<<path.at(i).y<<endl;
			}
			cout<<"[Press Key to go]"<<endl;
			char ch;
			cin>>ch;
			for(int i=0;i<path.size();i++){
				shared_ptr<Marker> marker;
				bool found=false;
				for(int i=0;i<10;i++){
					IplImage *img=cvQueryFrame(capture2);
					found=findAMarker(img,MIDBOT_MARKER,marker);
					if(found)break;
				}
				if(found){
					int direction;
					if(i<path.size()-1){
					if(path.at(i+1).y<path.at(i).y)direction=0;
					if(path.at(i+1).y>path.at(i).y)direction=2;
					if(path.at(i+1).x<path.at(i).x)direction=1;
					if(path.at(i+1).x>path.at(i).x)direction=3;
					}
					runAStep(marker,path.at(i),imageToMap2);
					if(i!=path.size()-1)capAndCal(capture2,imageToMap2,direction);
					cout<<"cal"<<endl;
					if(detectObstacle()){
						cout<<"obstacle detected"<<endl;
						obstacles.push_back(path.at(i+1));
						cvQueryFrame(capture2);
						cvQueryFrame(capture2);
						cvQueryFrame(capture2);
						cvQueryFrame(capture);
						cvQueryFrame(capture);
						cvQueryFrame(capture);
						TurnLeft(1);
						break;
					}
					cout<<"detect"<<endl;

					if(i!=path.size()-1)capAndCal(capture2,imageToMap2,direction);
					cout<<"cal"<<endl;
					if(i==path.size()-1)reach=true;
				}
				cvQueryFrame(capture2);
				cvQueryFrame(capture2);
				cvQueryFrame(capture2);

			}
		}
		//detectObstacle();
	}
	
	if(event==CV_EVENT_MBUTTONDOWN){//cross space path planning
		//find path to exit
		GridMap gridmap1(4,5);
		buildGridMap(cvQueryFrame(capture2),gridmap1,imageToMap,PATH_CROSSMODE);
		gridmap1.printContent();
		gridmap1.findPath();
		vector<CvPoint> path;
		gridmap1.getPath(path);
		if(gridmap1.isValidMap())cout<<"[Path]"<<endl;
		else cout<<"path not found";
		for(int i=0;i<path.size();i++){
			cout<<path.at(i).x<<","<<path.at(i).y<<endl;
		}

		for(int i=0;i<path.size()+3;i++){
			shared_ptr<Marker> marker;
			bool found=false;
			for(int i=0;i<10;i++){
				IplImage *img=cvQueryFrame(capture2);
				found=findAMarker(img,MIDBOT_MARKER,marker);
				if(found)break;
			}
			if(found){
				if(i<path.size())runAStep(marker,path.at(i),imageToMap);
				else calibrateAngle(marker,imageToMap,2);
			}
			cvQueryFrame(capture2);
			cvQueryFrame(capture2);
			cvQueryFrame(capture2);
		
		}
		//from exit to other space
		cout<<"go across spaces"<<endl;
		goToAPoint(MAP_TO_MAP2_ANGLE,MAP_TO_MAP2_DISTANCE);
		//find path to target
		capture2=cvCreateFileCapture("http://192.168.1.104/video.mjpg");
		GridMap gridmap2(3,3);

		buildGridMap(cvQueryFrame(capture2),gridmap2,imageToMap2,PATH_SINGLEMODE);
		gridmap2.printContent();
		gridmap2.findPath();
		path.clear();
		gridmap2.getPath(path);
		if(gridmap2.isValidMap())cout<<"[Path]"<<endl;
		else cout<<"path not found";
		for(int i=0;i<path.size();i++){
			cout<<path.at(i).x<<","<<path.at(i).y<<endl;
		}

		for(int i=0;i<path.size();i++){
			shared_ptr<Marker> marker;
			bool found=false;
			for(int i=0;i<10;i++){
				capture2=cvCreateFileCapture("http://192.168.1.104/video.mjpg");
				IplImage *img=cvQueryFrame(capture2);
				found=findAMarker(img,MIDBOT_MARKER,marker);
				if(found)break;
			}
			if(found){
				runAStep(marker,path.at(i),imageToMap2);
			}
		}
	}
}


int main(){



	capture=cvCaptureFromCAM(1);
	IplImage * capimg,*mapimg=cvCreateImage(cvSize(MAP_WIDTH,MAP_HEIGHT),IPL_DEPTH_8U,3);	
	cvNamedWindow ("cam1", CV_WINDOW_AUTOSIZE);
	cvNamedWindow ("map1", CV_WINDOW_AUTOSIZE);
	cvSetMouseCallback("map1",onMouse,0);
	int cornerids[4]={LEFTTOP_CORNER,RIGHTTOP_CORNER,LEFTBUTTON_CORNER,RIGHTBUTTON_CORNER};
	imageToMap=cvCreateMat (3, 3, CV_32FC1);
	//findPerspectiveMatrix(capture,cornerids,imageToMap,MAP_WIDTH,MAP_HEIGHT);

	////multiple cameras for same space and for obstacle avoidance, to enable this support ,just uncomment codes below
	capture2=cvCaptureFromCAM(0);
	//IplImage * capimg2,*mapimg2=cvCreateImage(cvSize(MAP_WIDTH,MAP_HEIGHT),IPL_DEPTH_8U,3);	
	//cvNamedWindow ("cam2", CV_WINDOW_AUTOSIZE);
	//cvNamedWindow ("map2", CV_WINDOW_AUTOSIZE);
	imageToMap2=cvCreateMat (3, 3, CV_32FC1);
	findPerspectiveMatrix(capture2,cornerids,imageToMap2,MAP_WIDTH,MAP_HEIGHT);
	
	//multiple spaces
	//capture2=cvCreateFileCapture("http://192168.1.104/video.mjpg");
	//IplImage * capimg2=cvCreateImage(cvSize.(MAP2_WIDTH,MAP2_HEIGHT),IPL_DEPTH_8U,3),*mapimg2=cvCreateImage(cvSize(MAP2_WIDTH,MAP2_HEIGHT),IPL_DEPTH_8U,3);	
	//imageToMap2=cvCreateMat (3, 3, CV_32FC1);
	//findPerspectiveMatrix(capture2,cornerids,imageToMap2,MAP2_WIDTH,MAP2_HEIGHT);

	//grab object and place it in the next grid
	//grabObject(OBJECT_MARKER,capture);
	while(1){
		capimg=cvQueryFrame(capture2);
		cvWarpPerspective (capimg,mapimg,imageToMap2);
		markerdetector(capimg,mapimg,imageToMap2);


		cvShowImage("cam1",capimg);
		cvShowImage("map1",mapimg);

		////multiple cameras for same space and multiple spaces, to enable this support ,just uncomment lines below



			
		if(cvWaitKey(30)==27)break;
	}
	return 0;
}

